#include "PIDControl.h"


//Pid控制
float PID_control_location(const float err, PID_location_Struct* PID)
{
    //比例
    float Pn = PID->p * err;
    //积分
    PID->Integral += err;
    float In = PID->Integral * PID->i;

    //微分
    float Dn = (err - PID->last_err) * PID->d;

    PID->last_err = err;
    //输出
    float Output = Pn + In + Dn;
    //限制幅度
    Output = Output > PID->Maxthreshold ? PID->Maxthreshold : Output;
    Output = Output < PID->Minthreshold ? PID->Minthreshold : Output;

    return Output;
}


float PID_control_increment(const float err, PID_increment_Struct* PID)
{
    //比例
    float Pn = PID->p * (err - PID->errk_1);
    //积分

    float In = err * PID->i;

    //微分
    float Dn = (err - 2 * PID->errk_1 + PID->errk_2) * PID->d;

    PID->errk_2 = PID->errk_1;
    PID->errk_1 = err;
    //输出
    PID->output += Pn + In + Dn;
    //限制幅度
    PID->output = PID->output > PID->Maxthreshold ? PID->Maxthreshold : PID->output;
    PID->output = PID->output < PID->Minthreshold ? PID->Minthreshold : PID->output;

    return  PID->output;
}
